function [X_dot] = plant(x,U,t)

global g I l m Jx Jy Jz
global a1 a2 a3
global b1 b2 b3
global kt kd 
X_dot = zeros(12,1);

Ux = cos(x(1))*sin(x(3))*cos(x(5))+sin(x(1))*sin(x(5));
Uy = cos(x(1))*sin(x(3))*sin(x(5))-sin(x(1))*cos(x(5));

omega1 = sqrt(abs(U(1)/(4*kt) + U(3)/(2*kt) + U(4)/(4*kd)));
omega2 = sqrt(abs(U(1)/(4*kt) - U(2)/(2*kt) - U(4)/(4*kd)));
omega3 = sqrt(abs(U(1)/(4*kt) - U(3)/(2*kt) + U(4)/(4*kd)));
omega4 = sqrt(abs(U(1)/(4*kt) + U(2)/(2*kt) - U(4)/(4*kd)));
omega = omega1 - omega2 + omega3 - omega4;

% D = (1 * rand - 0.5) * 0.4;
D = 0 ;
D_phi = 0.1*D; D_theta = 0.1*D; D_psi = 0.1*D;
% D = 0 ;
% D_phi = 0.1*D; D_theta = 0.1*D; D_psi = 0.1*D;
% Force0 = zeros(6,1);

% F_p0表示静止状态
% F_p0 = [F_px,F_py,F_pz]; % 无人机所给拉力，作用在x，y，z上
F_p0 = Force0(1:3);
% F_p0 = [0, 0, m_p/Quadrotor_Num];

% 计算tau需要知道物体位置和当前无人机位置
Tau_p0 = Force0(4:6);% 无人机作用力矩，作用在phi、theta、psi上 
Tau_p0 = zeros(1,3);
F_p0 = zeros(1,3);

X_dot(1) = x(2);
X_dot(2) = x(4)*x(6)*a1 + U(2)*b1 - I*x(4)*omega + D_phi - Tau_p0(1)/Jx;
X_dot(3) = x(4);
X_dot(4) = x(2)*x(6)*a2 + U(3)*b2 + I*x(2)*omega + D_theta - Tau_p0(2)/Jx;
X_dot(5) = x(6);
X_dot(6) = x(2)*x(4)*a3 + U(4)*b3 + D_psi - Tau_p0(3)/Jx;

% 位置状态
X_dot(7)  = x(8); 
X_dot(8)  = Ux*U(1)/m + D - F_p0(1)/m;
X_dot(9)  = x(10); 
X_dot(10) = Uy*U(1)/m + D - F_p0(2)/m;
X_dot(11) = x(12); 
X_dot(12) = -g + cos(x(1))*cos(x(3))*U(1)/m + D - F_p0(3)/m;
end 